import josx.platform.rcx.*;

public class Move
{

	static int FORWARD_CAL = -46;
	static int BACKWARD_CAL = 0;
	static int LEFT_CAL = -23;
	static int RIGHT_CAL = -23;

	public Move()
	{
		Motor.A.setPower(7);
		Motor.C.setPower(7);
	}

	public static void startSensor(int runtime)
	throws InterruptedException
	{
		int timer = 0;
		Sensor.S2.setTypeAndMode(1, 0x20);
		while (Sensor.S2.readValue() != 1 && timer < runtime)
		{
			timer += 10;
			Thread.sleep(10);
		}
		if (Sensor.S2.readValue() == 1)
			throw new InterruptedException();

	}


	public static boolean forward(int dist)
	throws InterruptedException
	{
		// message
		// TextLCD.print("FOWARD");
		// drive forward
		Motor.A.forward();
		Motor.C.forward();
		try
		{
			startSensor(dist * (2800 + FORWARD_CAL * 10));
		}
		catch (InterruptedException e)
		{
			Motor.A.stop();
			Motor.C.stop();
			Motor.A.backward();
			Motor.C.backward();
			Thread.sleep(500);

			Motor.A.stop();
			Motor.C.stop();
			Thread.sleep(100);

			return false;
		}

		Motor.A.stop();
		Motor.C.stop();
		Thread.sleep(100);

		return true;
	}

	public static boolean backward(int dist)
	throws InterruptedException
	{
		// message
		// TextLCD.print("BACKWARD");
		// drive forward
		Motor.A.backward();
		Motor.C.backward();
		Thread.sleep(dist * (2800 + BACKWARD_CAL * 10));
		Motor.A.stop();
		Motor.C.stop();
		Thread.sleep(100);

		return true;
	}

	public static boolean right()
	throws InterruptedException
	{
		// message
		// TextLCD.print("RIGHT");
		// drive forward
		Motor.A.forward();
		Motor.C.backward();
		Thread.sleep(1800 + RIGHT_CAL * 10);
		Motor.A.stop();
		Motor.C.stop();
		Thread.sleep(100);

		return true;
	}


	public static boolean left()
	throws InterruptedException
	{
		// message
		// TextLCD.print("LEFT");
		// drive forward
		Motor.A.backward();
		Motor.C.forward();
		Thread.sleep(1800 + LEFT_CAL * 10);
		Motor.A.stop();
		Motor.C.stop();
		Thread.sleep(100);

		return true;
	}

	public static void goal()
		throws InterruptedException
	{
		TextLCD.print("GOAL");
		Sound.beepSequence();
		Thread.sleep(1500);

	}

	public static void nogoal()
		throws InterruptedException
	{
		TextLCD.print("FAIL");
		Sound.buzz();
		Thread.sleep(1500);

	}

	public static void main(String args[])
		throws InterruptedException
	{
		//Move.forward(1);
		//Move.left();
		//Move.right();
		Move.nogoal();
	}
} // class Move
